package org.client.iot.ble;

import java.io.ByteArrayOutputStream;

public class ModBusTemplate {
    private static final char[] HEX_ARRAY = "0123456789ABCDEF".toCharArray();
    public static byte[] createModbusCommand(int slaveAddress, int functionCode, int startAddress, int registerCount) {
        ByteArrayOutputStream outputStream = new ByteArrayOutputStream();

        // 基础数据段
        outputStream.write(slaveAddress);          // 从机地址 0x01
        outputStream.write(functionCode);          // 功能码 0x03
        outputStream.write((startAddress >> 8) & 0xFF);  // 起始地址高位
        outputStream.write(startAddress & 0xFF);         // 起始地址低位
        outputStream.write((registerCount >> 8) & 0xFF);  // 寄存器数量高位
        outputStream.write(registerCount & 0xFF);        // 寄存器数量低位

        // 计算CRC16校验（Modbus专用）
        byte[] data = outputStream.toByteArray();
        int crc = calculateCRC16(data);
        outputStream.write(crc & 0xFF);            // CRC低位在前
        outputStream.write((crc >> 8) & 0xFF);     // CRC高位在后

        return outputStream.toByteArray();
    }
    // CRC16计算算法需要定义为同一类的成员方法
    private static int calculateCRC16(byte[] data) {
        int crc = 0xFFFF;
        for (byte b : data) {
            crc ^= (b & 0xFF);
            for (int i = 0; i < 8; i++) {
                if ((crc & 0x0001) != 0) {
                    crc >>= 1;
                    crc ^= 0xA001;
                } else {
                    crc >>= 1;
                }
            }
        }
        return crc;
    }

    public static String bytesToHex(byte[] bytes) {
        char[] hexChars = new char[bytes.length * 2];
        for (int i = 0; i < bytes.length; i++) {
            int v = bytes[i] & 0xFF; // 处理负数问题
            hexChars[i * 2] = HEX_ARRAY[v >>> 4];
            hexChars[i * 2 + 1] = HEX_ARRAY[v & 0x0F];
        }
        return new String(hexChars);
    }
}
